/*
 * Copyright (c) 2022, www.lgmgim.cn
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *	* Redistributions of source code must retain the above copyright
 * 	  notice, this list of conditions and the following disclaimer.
 * 	* Redistributions in binary form must reproduce the above copyright
 *	  notice, this list of conditions and the following disclaimer in the
 * 	  documentation and/or other materials provided with the distribution.
 * 	* Neither the name of the University of California, Berkeley nor the
 * 	  names of its contributors may be used to endorse or promote products
 * 	  derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
/*
 * @file charge.cpp
 * @author YinWeibin
 * @date 1 15, 2024
 * @brief charger operation
 * Contact: Weibin.yin@lgmgim.cn
 */
#include "task_manager/task_manager.h"
#include "task_manager/service_manager.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
#include <shared_parameters/shared_params_interface.hpp>

TaskManager::TaskManager(const std::shared_ptr<rclcpp::Node> node): node_(node)
{
	std::string package_share_dir = ament_index_cpp::get_package_share_directory("agv_dispatch_comm");
    msg_path_ = package_share_dir + "/messages/";
    schemas_path_ = package_share_dir + "/schemas/";
	RCLCPP_INFO(node_->get_logger(), "Constructor entry");
	RCLCPP_INFO_STREAM(node_->get_logger(), "msg path:" << msg_path_ << ",schemas path:" << schemas_path_);
	
    initParameters();
	// Load configuration
	loadInitialState();
	getParam(); 

	initMQTT();
	
	RCLCPP_INFO(node_->get_logger(), "mqtt client set_callback");
	// Initialize ROS subscribers and publishers
	initializeROSComponents();
	
	// Connect to MQTT broker
	connectToMQTTBroker();
	RCLCPP_INFO(node_->get_logger(), "mqtt connect over");
	
	subTopic();	

	main_executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
    main_executor_->add_node(node_);
}

void TaskManager::run() {
	main_executor_->spin();
}

bool TaskManager::loadConfig(const std::string& config_path) {
	std::fstream fin(config_path.c_str(), std::ios::in);
  
    if (!fin) {
        RCLCPP_ERROR(node_->get_logger(), "could not open %s.", config_path.c_str());
        return false;
    }

    try {
        YAML::Node yamlConfig = YAML::Load(fin);
        fin.close();
        if (!yamlConfig) {
            RCLCPP_ERROR(node_->get_logger(), "read %s failed. data is not yaml format", config_path.c_str());
            return false;
        }
        if (yamlConfig["msg_path"] && yamlConfig["schemas_path"]) {
            std::string home = (std::getenv("HOME") ? std::getenv("HOME") : "");
            msg_path_ = home + yamlConfig["msg_path"].as<std::string>();
            schemas_path_ = home + yamlConfig["schemas_path"].as<std::string>();
            RCLCPP_INFO(node_->get_logger(), "[loadConfig] Config loaded successfully");
        } else {
            RCLCPP_ERROR(node_->get_logger(), "read failed. data is not exist");
            return false;
        }
    } catch (const json::parse_error &e) {
        RCLCPP_ERROR(node_->get_logger(), "[loadConfig] JSON error (byte %zu): %s", e.byte, e.what());
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "[loadConfig] Unexpected error: %s", e.what());
    }
    return true;
}

void TaskManager::loadInitialState() {
    std::ifstream json_file(msg_path_ + "state_msg.json");
    if (!json_file.is_open()) {
        RCLCPP_ERROR(node_->get_logger(), "Failed to open state file: %sstate_msg.json", msg_path_.c_str());
        return;
    }
    RCLCPP_INFO_STREAM(node_->get_logger(), "Version: V" << NLOHMANN_JSON_VERSION_MAJOR << "." << NLOHMANN_JSON_VERSION_MINOR);
    try {
        json_file >> json_object_;
        RCLCPP_INFO(node_->get_logger(), "json object init success");
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "Failed to parse state file: %s", e.what());
    }
    // File will be closed automatically when json_file goes out of scope
}

void TaskManager::getParam() {
	// Get parameters from ROS parameter server
    node_->get_parameter_or("broker_ip", broker_ip_, std::string("127.0.0.1"));
    node_->get_parameter_or("broker_port", broker_port_, 1883);
    node_->get_parameter_or("netInterval", netInterval_, 15);
    char* robot_id = std::getenv("ROBOT_ID");
    agv_num_ = (robot_id ? std::string(robot_id) : "default_robot");
    RCLCPP_INFO(node_->get_logger(), "[getParam]: broker_ip:[%s],port:[%d],netInterval:[%d],agvID:[%s]", 
                broker_ip_.c_str(), broker_port_, netInterval_, agv_num_.c_str());
}

void TaskManager::initializeROSComponents() {
	// Subscribers
	state_sub_ = node_->create_subscription<agv_msgs::msg::State>(
	    "vda5050_state", 10, 
	    [this](agv_msgs::msg::State::ConstSharedPtr msg) {
	        this->stateCB(msg);
	    });
	agv_mode_sub_ = node_->create_subscription<agv_msgs::msg::AGVMode>(
	    "agvmode", 10, 
	    [this](agv_msgs::msg::AGVMode::ConstSharedPtr msg) {
	        this->agvModeCB(msg);
	    });
	battery_info_sub_ = node_->create_subscription<agv_msgs::msg::BatInfo>(
	    "batteryInfo", 10, 
	    [this](agv_msgs::msg::BatInfo::ConstSharedPtr msg) {
	        this->batteryInfoCB(msg);
	    });
	map_sub_ = node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
	    "/map", 10,
	    [this](nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) {
	        this->mapCB(msg);
	    });
	switch_sub_ = node_->create_subscription<agv_msgs::msg::PointCloudPubSwitch>(
	    "/point_cloud_switch", 10,
	    [this](agv_msgs::msg::PointCloudPubSwitch::ConstSharedPtr msg) {
	        this->switchCB(msg);
	    });
	event_status_sub_ = node_->create_subscription<agv_msgs::msg::AGVEventStatus>(
	    "/eventstatus", 10,
	    [this](agv_msgs::msg::AGVEventStatus::ConstSharedPtr msg) {
	        this->eventCB(msg);
	    });
	periph_status_sub_ = node_->create_subscription<agv_msgs::msg::PeriphStatus>(
	    "/periphStatus", 10,
	    [this](agv_msgs::msg::PeriphStatus::ConstSharedPtr msg) {
	        this->periphStatusCB(msg);
	    });
	agv_event_sub_ = node_->create_subscription<agv_msgs::msg::AGVEvent>(
	    "/agvevent", 10,
	    [this](agv_msgs::msg::AGVEvent::ConstSharedPtr msg) {
	        this->agvEventCB(msg);
	    });
	auto_status_sub_ = node_->create_subscription<agv_msgs::msg::AgvAutoStatus>(
	    "/auto_run_status", 10,
	    [this](agv_msgs::msg::AgvAutoStatus::ConstSharedPtr msg) {
	        this->agvStatusCB(msg);
	    });
	diagnosis_sub_ = node_->create_subscription<agv_msgs::msg::Diagnosis>(
	    "/statistics_data", 10,
	    [this](agv_msgs::msg::Diagnosis::ConstSharedPtr msg) {
	        this->agvDiagnosisCB(msg);
	    });

    // Publish
    connection_pub_ = node_->create_publisher<agv_msgs::msg::ConnectionMonitor>("connection_state", 10);
    pc_switch_pub_ = node_->create_publisher<agv_msgs::msg::PointCloudPubSwitch>("/point_cloud_switch", 1);
    error_pub_ = node_->create_publisher<agv_msgs::msg::AGVEvent>("/agvevent", 1); 

    // Service
    order_client_ = node_->create_client<agv_srvs::srv::Order>("vda5050_order");
    instant_actions_client_ = node_->create_client<agv_srvs::srv::InstantActions>("/vda5050_instantActions");
}

void TaskManager::initMQTT() {
	try {
        std::string client_id = "Task Manager" + agv_num_;
        std::string server_uri = "mqtt://" + broker_ip_ + ":" + std::to_string(broker_port_);
        mqtt_client_ = std::make_unique<mqtt::async_client>(server_uri, client_id);
        mqtt_client_->set_callback(*this);
    } catch (const mqtt::exception& e) {
        RCLCPP_ERROR(node_->get_logger(), "MQTT initialization failed: %s", e.what());
        rclcpp::shutdown();
    }
}

void TaskManager::connectToMQTTBroker() {
	// using namespace std::chrono_literals;
	mqtt::connect_options connOpts;
    connOpts.set_keep_alive_interval(20);
    connOpts.set_clean_session(true);
    connOpts.set_automatic_reconnect(true);
   
    try {
        mqtt::token_ptr conntok = mqtt_client_->connect(connOpts);
        
        conntok->wait();
        if (conntok->get_return_code() != mqtt::SUCCESS) {
            RCLCPP_ERROR(node_->get_logger(), "MQTT connection failed!");
            while (rclcpp::ok()) {
                RCLCPP_ERROR(node_->get_logger(), "MQTT reconnecting!");
                try {
                    std::this_thread::sleep_for(std::chrono::seconds(1));
                    conntok = mqtt_client_->reconnect();
                    if (conntok->get_return_code() == mqtt::SUCCESS) {
                        break;
                    }
                } catch (...) {
                    RCLCPP_ERROR(node_->get_logger(), "reconnect except");
                }
            }
        }
        // 启动后台线程
        mqtt_client_->start_consuming();
        RCLCPP_INFO(node_->get_logger(), "MQTT connected and loop started");
    } catch (const mqtt::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "MQTT initialization failed: %s", e.what());
        rclcpp::shutdown();
    }
}

void TaskManager::initializeTimers() {
	publish_state_timer_ = node_->create_wall_timer(std::chrono::milliseconds(200),
        std::bind(&TaskManager::publishState, this));
    publish_order_id_timer_ = node_->create_wall_timer(std::chrono::seconds(1),
        std::bind(&TaskManager::publishOrderId, this));
    state_json_timer_ = node_->create_wall_timer(std::chrono::milliseconds(100),
        std::bind(&TaskManager::stateJSON, this));
    // publish_point_cloud_timer_  publish_map_timer_
    publish_error_timer_ = node_->create_wall_timer(std::chrono::seconds(1),
        std::bind(&TaskManager::publishError, this));
    publish_connection_state_timer_ = node_->create_wall_timer(std::chrono::seconds(1),
        std::bind(&TaskManager::publishConnectionState, this));
}

// Callback methods
void TaskManager::stateCB(agv_msgs::msg::State::ConstSharedPtr msg) {
	std::lock_guard<std::mutex> lock(state_mutex_);
    state_.orderId = msg->orderid;
    state_.orderUpdateId = msg->orderupdateid;
    state_.lastNodeId = msg->lastnodeid;
    state_.lastNodeSequenceId = std::to_string(msg->lastnodesequenceid);
    state_.agvPosition.position.x = msg->agvposition.x;
    state_.agvPosition.position.y = msg->agvposition.y;
    state_.agvPosition.position.theta = msg->agvposition.theta;
    state_.agvPosition.localizationScore = msg->agvposition.localizationscore;
    state_.agvPosition.position.mapId = msg->agvposition.mapid;
    state_.velocity["vx"] = msg->velocity.vx;
    state_.velocity["vy"] = msg->velocity.vy;
    state_.velocity["omega"] = msg->velocity.omega;
    state_.driving = msg->driving;
    state_.paused = msg->paused;
    state_.distanceSinceLastNode = msg->distancesincelastnode;
    state_.actionStates.clear();

    for (const auto &aState : msg->actionstates) {
        ActionState actionState;
        actionState.actionId = aState.actionid;
        actionState.actionType = aState.actiontype;
        actionState.actionStatus = aState.actionstatus;
        actionState.actionDescription = aState.actiondescription;
        actionState.resultDescription = aState.resultdescription;
        state_.actionStates.push_back(actionState);
    }
    state_.edgeStates.clear();
    for (const auto &edgeState : msg->edgestates) {
        EdgeState es;
        es.edgeId = edgeState.edgeid;
        es.sequenceId = std::to_string(edgeState.sequenceid);
        es.released = edgeState.released;
        state_.edgeStates.push_back(es);
    }
}

void TaskManager::agvModeCB(agv_msgs::msg::AGVMode::ConstSharedPtr msg) {
	std::lock_guard<std::mutex> lock(state_mutex_);
    switch (msg->mode) {
        case agv_msgs::msg::AGVMode::MANUAL:
            state_.operatingMode = MANUAL_OP_MODE;
            break;
        case agv_msgs::msg::AGVMode::SEMI_AUTOMATIC:
            state_.operatingMode = SEMIAUTOMATIC_OP_MODE;
            break;
        case agv_msgs::msg::AGVMode::AUTOMATIC:
            state_.operatingMode = AUTOMATIC_OP_MODE;
            break;
        case agv_msgs::msg::AGVMode::LOCAL_AUTOMATIC:
            state_.operatingMode = TEACHIN_OP_MODE;
            break;
    }
}

void TaskManager::batteryInfoCB(agv_msgs::msg::BatInfo::ConstSharedPtr msg) {
	std::lock_guard<std::mutex> lock(state_mutex_);
    state_.batteryStates.batteryCharge = msg->numeric_bat;
    state_.batteryStates.charging = msg->charge_flag;
    state_.batteryStates.batteryVoltage = msg->voltage;
    state_.batteryStates.chargeFinished = msg->charge_finished;
}

// void TaskManager::scanCB(const sensor_msgs::PointCloud2::SharedPtr& msg) {
// 	std::lock_guard<std::mutex> lock(point_cloud_mutex_);
	
// 	point_list_.clear();
// 	point_msg_["stamp"] = msg->header.stamp.sec;
	
// 	// Process point cloud data
// 	for (const auto& point : sensor_msgs::PointCloud2Iterator<float>(*msg, "x")) {
// 		json point_;
// 		point_["x"] = point[0];
// 		point_["y"] = point[1];
// 		point_list_.push_back(point_);
// 	}
	
// 	point_msg_["points"] = point_list_;
// }

void TaskManager::eventCB(agv_msgs::msg::AGVEventStatus::ConstSharedPtr msg) {
	std::lock_guard<std::mutex> lock(state_mutex_);
    Errors error;
    error.errorType = std::to_string(msg->ecode);
    error.errorDescription = msg->description;
    state_.errors.clear();
    state_.errors.push_back(error);
    if (!description_.empty()) {
        Errors desc_error;
        desc_error.errorType = "0";
        desc_error.errorDescription = description_;
        state_.errors.push_back(desc_error);
    }
}

void TaskManager::periphStatusCB(agv_msgs::msg::PeriphStatus::ConstSharedPtr msg) {
	std::lock_guard<std::mutex> lock(state_mutex_);
	
	state_.loads.clear();
	for (const auto& load : msg->loads) {
		Load l;
		l.loadPosition = load.loadposition;
		l.loadId = load.loadid;
		l.loadType = load.loadtype.empty() ? "-1" : load.loadtype;
		state_.loads.push_back(l);
	}
}

void TaskManager::agvEventCB(agv_msgs::msg::AGVEvent::ConstSharedPtr msg) {
	if (msg->header.frame_id == "/qrodom") {
		description_ = msg->description;
	}
}

void TaskManager::agvStatusCB(agv_msgs::msg::AgvAutoStatus::ConstSharedPtr msg) {
	std::lock_guard<std::mutex> lock(order_mutex_);
	
	auto now = std::chrono::system_clock::now();
	orderIdMsg_["orderIdNew"] = msg->orderidnew;
	orderIdMsg_["orderUpdateIdNew"] = msg->orderupdateidnew;
	orderIdMsg_["nodeIdNew"] = msg->nodeidnew;
	
	// ROS_INFO_THROTTLE(0.5, "receive order finish! take %ld ms", 
	// 					std::chrono::duration_cast<std::chrono::milliseconds>(now - t_old_).count());
	t_old_ = now;
}

void TaskManager::agvDiagnosisCB(agv_msgs::msg::Diagnosis::ConstSharedPtr msg) {
	std::lock_guard<std::mutex> lock(state_mutex_);
	
	state_.center_mileage = msg->center_mileage;
	state_.online_time = msg->online_time;
	state_.move_time = msg->move_time;
	state_.fault_time = msg->fault_time;
	state_.order_counts = msg->order_counts;
	state_.discharge_time = msg->discharge_time;
	state_.charge_counts = msg->charge_counts;
	state_.charge_time = msg->charge_time;
}

void TaskManager::mapCB(nav_msgs::msg::OccupancyGrid::ConstSharedPtr msg) {
	if (!map_pub_switch_ || navigation_mode_ != 0) {
		return;
	}
	
	std::lock_guard<std::mutex> lock(map_mutex_);
	
	map_msg_["stamp"] = msg->header.stamp.sec;
	map_msg_["resolution"] = msg->info.resolution;
	map_msg_["width"] = msg->info.width;
	map_msg_["height"] = msg->info.height;
	map_msg_["origin"]["position"]["x"] = msg->info.origin.position.x;
	map_msg_["origin"]["position"]["y"] = msg->info.origin.position.y;
	
	// Process map data
	std::string map_info_bytes = map_msg_.dump() + "#%#";
	
	// Convert map data to image
	cv::Mat img(msg->info.height, msg->info.width, CV_8UC1);
	std::memcpy(img.data, &msg->data[0], msg->data.size());
	cv::flip(img, img, 0);
	
	std::vector<uchar> buf;
	cv::imencode(".png", img, buf, {cv::IMWRITE_PNG_COMPRESSION, 5});
	
	// Combine map info and image data
	send_info_ = map_info_bytes + std::string(buf.begin(), buf.end());
}

void TaskManager::switchCB(agv_msgs::msg::PointCloudPubSwitch::ConstSharedPtr msg) {
	pc_pub_switch_ = msg->pc_pub_switch;
	map_pub_switch_ = msg->map_pub_switch;
}

// Timer callback methods
void TaskManager::publishState() {
	if (mqtt_client_ && mqtt_client_->is_connected() && !heart_bit_state_.is_null()) {
		mqtt_client_->publish("/" + agv_num_ + "/" + STATE_TOPIC, heart_bit_state_);
		// ROS_INFO("mqtt connected,publish State");
	}
	else
		RCLCPP_WARN(node_->get_logger(), "mqtt disconnected");
}

void TaskManager::publishConnectionState() {
	auto now = std::chrono::system_clock::now();
    if (std::chrono::duration_cast<std::chrono::seconds>(now - time_monitor_old_).count() >= netInterval_) {
        RCLCPP_INFO(node_->get_logger(), "connection to master failed!!!");
        master_disconnect_ = true;
    } else {
        master_disconnect_ = false;
    }
    agv_msgs::msg::ConnectionMonitor connection_state;
    connection_state.brokerconnnection = broker_disconnect_;
    connection_state.masterconnnection = master_disconnect_;
    connection_pub_->publish(connection_state);
}

void TaskManager::publishOrderId() {
	try {
        std::string idMsg = orderIdMsg_.dump();
        if (mqtt_client_ && mqtt_client_->is_connected()) {
            mqtt_client_->publish("/" + agv_num_ + "/" + ORDER_ID_TOPIC, idMsg);
        }
        RCLCPP_INFO(node_->get_logger(), "pub orderId at time %.2f", node_->now().seconds());
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "%s", e.what());
    }
}

void TaskManager::stateJSON() {
	std::lock_guard<std::mutex> lock(state_mutex_);
	
	json_object_["headerId"] = "null";
	json_object_["orderId"] = state_.orderId;
	json_object_["orderUpdateId"] = std::to_string(state_.orderUpdateId);
	json_object_["manufacturer"] = "lgmg";
	json_object_["serialNumber"] = agv_num_;
	
	auto now = std::chrono::system_clock::now();
	auto in_time_t = std::chrono::system_clock::to_time_t(now);
	std::stringstream ss;
	ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%dT%H:%M:%S");
	json_object_["timestamp"] = ss.str();
	
	json_object_["lastNodeId"] = state_.lastNodeId;
	json_object_["lastNodeSequenceId"] = state_.lastNodeSequenceId;
	json_object_["driving"] = state_.driving;
	json_object_["operatingMode"] = state_.operatingMode;
	json_object_["velocity"] = state_.velocity;
	json_object_["agvPosition"] = state_.agvPosition;
	json_object_["paused"] = state_.paused;
	json_object_["distanceSinceLastNode"] = state_.distanceSinceLastNode;
	json_object_["actionStates"] = state_.actionStates;
	json_object_["edgeStates"] = state_.edgeStates;
	json_object_["newBaseRequest"] = state_.newBaseRequest;
	json_object_["errors"] = state_.errors;
	json_object_["batteryState"] = state_.batteryStates;
	json_object_["loads"] = state_.loads;
	json_object_["centerMileage"] = state_.center_mileage;
	json_object_["onlineTime"] = state_.online_time;
	json_object_["moveTime"] = state_.move_time;
	json_object_["faultTime"] = state_.fault_time;
	json_object_["orderCounts"] = state_.order_counts;
	json_object_["dischargeTime"] = state_.discharge_time;
	json_object_["chargeCounts"] = state_.charge_counts;
	json_object_["chargeTime"] = state_.charge_time;
	
	try {
		heart_bit_state_ = json_object_.dump();
	} catch (const std::exception& e) {
		RCLCPP_ERROR(node_->get_logger(), "json convert failed: %s", e.what());
	}
}

void TaskManager::publishPointCloud() {
	if (!pc_pub_switch_) {
        return;
    }
    try {
        std::string publish_data = point_msg_.dump();
        if (mqtt_client_ && mqtt_client_->is_connected()) {
            mqtt_client_->publish("/" + agv_num_ + "/" + POINT_CLOUD_TOPIC, publish_data);
        }
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "convert points to json file failed: %s", e.what());
    }
}

void TaskManager::publishMap() {
    node_->get_parameter_or("/localization", navigation_mode_, 1);
    if (map_pub_switch_ && navigation_mode_ == 0) {
        if (mqtt_client_ && mqtt_client_->is_connected()) {
            mqtt_client_->publish("/" + agv_num_ + "/" + MAP_TOPIC, send_info_);
        }
    } else {
        std::string error_msg = "map date is only published in mapping mode!";
        if (mqtt_client_ && mqtt_client_->is_connected()) {
            mqtt_client_->publish("/" + agv_num_ + "/" + MAP_TOPIC, error_msg);
        }
    }
}

void TaskManager::publishError() {
	agv_msgs::msg::AGVEvent event_msg;
    event_msg.header.stamp = node_->now();
    event_msg.header.frame_id = node_->get_name();
    event_msg.ecode = 0;
    event_msg.type = 0;
    event_msg.action = 0;
    event_msg.eventtime = node_->now().seconds();
    if (state_.operatingMode != MANUAL_OP_MODE) {
        if (broker_disconnect_) {
            event_msg.description = "broker connect error!";
            event_msg.ecode = agv_msgs::msg::AGVEvent::NETWORK_FAIL_EVENT;
            event_msg.type = 2;
            event_msg.action = 3;
        } else if (master_disconnect_) {
            event_msg.description = "master connect error!";
            event_msg.ecode = agv_msgs::msg::AGVEvent::NETWORK_FAIL_EVENT;
            event_msg.type = 2;
            event_msg.action = 3;
        }
    }
    error_pub_->publish(event_msg);
}

// Helper methods
std::string TaskManager::getEnvVar(const std::string& key) {
	const char* val = std::getenv(key.c_str());
	return val ? std::string(val) : "";
}

// MQTT callback implementation
void TaskManager::message_arrived(mqtt::const_message_ptr msg) {
	std::string topic = msg->get_topic();
    std::string payload = msg->to_string();
    if (payload.empty()) {
        RCLCPP_WARN(node_->get_logger(), "Received empty message on topic: %s", topic.c_str());
        return;
    }
    try {
        json json_data = json::parse(payload);
        if (topic == "/" + agv_num_ + "/" + ORDER_TOPIC) {
            orderHandle(json::parse(payload));
        } else if (topic == "/" + agv_num_ + "/" + INSTANT_ACTION) {
            instantActionHandle(json::parse(payload));
        } else if (topic == "/" + agv_num_ + "/" + CONNECTION_TOPIC) {
            connectionHandle(json::parse(payload));
        } 
        // 其它 topic 可根据需要添加处理
        else {
            RCLCPP_INFO(node_->get_logger(), "topic is wrong! please check!!!");
        }
    } catch (const json::parse_error &e) {
        RCLCPP_ERROR(node_->get_logger(), "JSON parse error on topic %s: %s", topic.c_str(), e.what());
        RCLCPP_ERROR(node_->get_logger(), "Invalid payload: %s", payload.c_str());
    } catch (const std::exception &e) {
        RCLCPP_ERROR(node_->get_logger(), "Error processing MQTT message: %s", e.what());
    }
	
}

void TaskManager::connection_lost(const std::string& cause) {
	RCLCPP_WARN(node_->get_logger(), "Connection lost: %s", cause.c_str());
    broker_disconnect_ = true;
}

void TaskManager::subTopic() {
 	if (!mqtt_client_->is_connected()) {
        RCLCPP_ERROR(node_->get_logger(), "MQTT client not connected!");
        return;
    }
    if (agv_num_.empty()) {
        RCLCPP_ERROR(node_->get_logger(), "AGV ID is empty!");
        return;
    }
    std::vector<std::string> topics = {
        "/" + agv_num_ + "/" + ORDER_TOPIC,
        "/" + agv_num_ + "/" + INSTANT_ACTION,
        "/" + agv_num_ + "/" + CONNECTION_TOPIC,
        "/" + agv_num_ + "/" + LOCALIZATION_TOPIC,
        "/" + agv_num_ + "/" + QR_LOCALIZATION_REQUEST_TOPIC,
        "/" + agv_num_ + "/" + POINT_CLOUD_SWITCH,
        "/" + agv_num_ + "/" + SAVE_MAP_TOPIC,
        "/" + agv_num_ + "/" + NAV_MODE_TOPIC,
        "/" + agv_num_ + "/" + MAP_MODIFY_TOPIC,
        "/" + agv_num_ + "/" + UPDATEQR_TOPIC,
        "/" + agv_num_ + "/" + MAP_MERGE_TOPIC
    };
    RCLCPP_INFO(node_->get_logger(), "Attempting to subscribe to %zu topics...", topics.size());
    for (const auto &topic : topics) {
        int retry = 0;
        while (retry < 3) {
            try {
                mqtt::token_ptr subToken = mqtt_client_->subscribe(topic, 0);
                if (subToken->wait_for(std::chrono::seconds(3))) {
                    auto rc = subToken->get_return_code();
                    if (rc == mqtt::SUCCESS) {
                        RCLCPP_INFO(node_->get_logger(), "Subscribed to %s", topic.c_str());
                        break;
                    } else {
                        RCLCPP_ERROR(node_->get_logger(), "Subscribe failed: %s", MQTTReasonCode_toString(static_cast<MQTTReasonCodes>(rc)));
                    }
                } else {
                    RCLCPP_ERROR(node_->get_logger(), "Subscribe timeout for %s", topic.c_str());
                }
            } catch (const mqtt::exception &e) {
                RCLCPP_ERROR(node_->get_logger(), "MQTT error: %s", e.what());
            }
            retry++;
            if (retry < 3)
                std::this_thread::sleep_for(std::chrono::seconds(1));
        }
    }

    broker_disconnect_ = false;
    initializeTimers();
}

void TaskManager::connected(const std::string& tok) {
	RCLCPP_INFO(node_->get_logger(), "mqtt connected!!");
}

// MQTT message handlers
void TaskManager::orderHandle(const json& order) {
	changeOrder(order);
}

void TaskManager::changeOrder(const json& order) {
	state_.headerId = header_id_;
    state_.manufacturer = order["manufacturer"].get<std::string>();
    auto request = std::make_shared<agv_srvs::srv::Order::Request>();
    request->order.zonesetid = order["zoneSetId"].get<std::string>();
    request->order.timestamp = order["timestamp"].get<std::string>();
    request->order.serialnumber = order["serialNumber"].get<std::string>();
    request->order.orderid = order["orderId"].get<std::string>();
    request->order.orderupdateid = order["orderUpdateId"].get<int>();
    for (const auto &node : order["nodes"]) {
        agv_msgs::msg::MapNode nd;
        nd.nodeid = node["nodeId"].get<std::string>();
        nd.sequenceid = node["sequenceId"].get<unsigned int>();
        nd.released = node["released"].get<bool>();
        for (const auto &action : node["actions"]) {
            agv_msgs::msg::Action at;
            at.actiontype = action["actionType"].get<std::string>();
            at.actionid = action["actionId"].get<std::string>();
            at.blockingtype = action["blockingType"].get<std::string>();
            for (const auto &param : action["actionParameters"]) {
                agv_msgs::msg::ActionParam actionParam;
                actionParam.key = param["key"].get<std::string>();
                actionParam.value = param["value"].get<std::string>();
                at.actionparameters.push_back(actionParam);
            }
            nd.actions.push_back(at);
        }
        request->order.nodes.push_back(nd);
    }
 
	RCLCPP_INFO(node_->get_logger(), "\n[Order Request]\n"
			   "--------------------------------\n"
	           "\nTimestamp: %s"
	           "\nSerialNumber: %s"
	           "\nOrderID: %s"
	           "\nOrderUpdateID: %u"
	           "\nZoneSetID: %s",
	           request->order.timestamp.c_str(),
	           request->order.serialnumber.c_str(),
	           request->order.orderid.c_str(),
	           request->order.orderupdateid,
	           request->order.zonesetid.c_str());

	for (size_t i = 0; i < request->order.nodes.size(); ++i) {
	    const auto& node = request->order.nodes[i];
	    RCLCPP_INFO(node_->get_logger(), 
	               "\nNode[%zu]:"
	               "\n  NodeID: %s"
	               "\n  SequenceID: %u"
	               "\n  Description: %s"
	               "\n  Released: %s",
	               i,
	               node.nodeid.c_str(),
	               node.sequenceid,
	               node.nodedescription.c_str(),
	               node.released ? "true" : "false");

	    for (size_t j = 0; j < node.actions.size(); ++j) {
	        const auto& action = node.actions[j];
	        RCLCPP_INFO(node_->get_logger(),
	                   "  Action[%zu]:"
	                   "\n    Type: %s"
	                   "\n    ID: %s"
	                   "\n    Description: %s"
	                   "\n    BlockingType: %s",
	                   j,
	                   action.actiontype.c_str(),
	                   action.actionid.c_str(),
	                   action.actiondescription.c_str(),
	                   action.blockingtype.c_str());

	        for (size_t k = 0; k < action.actionparameters.size(); ++k) {
	            const auto& param = action.actionparameters[k];
	            RCLCPP_INFO(node_->get_logger(),
	                       "    Param[%zu]:"
	                       "\n      Key: %s"
	                       "\n      Value: %s"
	                       "\n      DataType: %s",
	                       k,
	                       param.key.c_str(),
	                       param.value.c_str(),
	                       param.valuedatatype.c_str());
	        }
	    }
	}

	RCLCPP_INFO(node_->get_logger(), "--------------------------------");

    ServiceCaller<agv_srvs::srv::Order>::CallAsync(
	    this->node_, 
	    order_client_,
	    request,
	    [this](const auto& result) {
	        if (result.success) {
	            RCLCPP_INFO(this->node_->get_logger(), "Success: %s", result.message.c_str());
	        } else {
	            RCLCPP_ERROR(this->node_->get_logger(), "Failed: %s", result.message.c_str());
	        }
	    }
	);

}

void TaskManager::instantActionHandle(const json& instant_order) {
	auto request = std::make_shared<agv_srvs::srv::InstantActions::Request>();
    for (const auto &action : instant_order["actions"]) {
        agv_msgs::msg::Action a;
        a.actiontype = action["actionType"].get<std::string>();
        a.actionid = action["actionId"].get<std::string>();
        a.blockingtype = action["blockingType"].get<std::string>();
        for (const auto &param : action["actionParameters"]) {
            agv_msgs::msg::ActionParam actionParam;
            actionParam.key = param["key"].get<std::string>();
            actionParam.value = param["value"].get<std::string>();
            a.actionparameters.push_back(actionParam);
        }
        request->actions.push_back(a);
    }

    RCLCPP_INFO(node_->get_logger(), "\nInstantActions Request Content:");
	RCLCPP_INFO(node_->get_logger(), "----------------------------------");
	RCLCPP_INFO(node_->get_logger(), "Source: %s", request->source.c_str());

	for (size_t i = 0; i < request->actions.size(); ++i) {
	    const auto& action = request->actions[i];
	    RCLCPP_INFO(node_->get_logger(), "\nAction[%zu]:", i);
	    RCLCPP_INFO(node_->get_logger(), "  Type: %s", action.actiontype.c_str());
	    RCLCPP_INFO(node_->get_logger(), "  ID: %s", action.actionid.c_str());
	    RCLCPP_INFO(node_->get_logger(), "  Description: %s", action.actiondescription.c_str());
	    RCLCPP_INFO(node_->get_logger(), "  Blocking Type: %s", action.blockingtype.c_str());

	    for (size_t j = 0; j < action.actionparameters.size(); ++j) {
	        const auto& param = action.actionparameters[j];
	        RCLCPP_INFO(node_->get_logger(), "  Parameter[%zu]:", j);
	        RCLCPP_INFO(node_->get_logger(), "    Key: %s", param.key.c_str());
	        RCLCPP_INFO(node_->get_logger(), "    Value: %s", param.value.c_str());
	        RCLCPP_INFO(node_->get_logger(), "    Data Type: %s", param.valuedatatype.c_str());
	    }
	}
	RCLCPP_INFO(node_->get_logger(), "----------------------------------");

	ServiceCaller<agv_srvs::srv::InstantActions>::CallAsync(
	    this->node_, 
	    instant_actions_client_,
	    request,
	    [this](const auto& result) {
	        if (result.success) {
	            RCLCPP_INFO(this->node_->get_logger(), "Success: %s", result.message.c_str());
	        } else {
	            RCLCPP_ERROR(this->node_->get_logger(), "Failed: %s", result.message.c_str());
	        }
	    }
	);

}

void TaskManager::connectionHandle(const json& connection_msg) {
	RCLCPP_INFO(node_->get_logger(), "connection msg recevice!!!");
    time_monitor_old_ = std::chrono::system_clock::now();
}

void TaskManager::localizationHandle(const json& localization_msg) {
	// Implementation of localization handling
}

void TaskManager::qrLocalizationHandle(const json& qr_localization_msg) {
	// Implementation of QR localization handling
}

void TaskManager::pcSwitchHandle(const json& pc_switch_msg) {
	agv_msgs::msg::PointCloudPubSwitch switch_msg;
    try {
        switch_msg.pc_pub_switch = pc_switch_msg["pc_pub_switch"];
        switch_msg.map_pub_switch = pc_switch_msg["map_pub_switch"];
        pc_switch_pub_->publish(switch_msg);
        RCLCPP_INFO(node_->get_logger(), "PC switch: %d, Map switch: %d",
                    switch_msg.pc_pub_switch, switch_msg.map_pub_switch);
    } catch (...) {
        RCLCPP_INFO(node_->get_logger(), "point cloud switch message is wrong!");
    }
}

void TaskManager::saveMapHandle(const json& save_map_msg) {
	// Implementation of save map handling
}

void TaskManager::navModeHandle(const json& nav_mode_msg) {
	// Implementation of navigation mode handling
}

void TaskManager::mapModifyHandle(const json& msg) {
	// Implementation of map modify handling
}

void TaskManager::updateQRHandle(const json& msg) {
	// Implementation of update QR handling
}

void TaskManager::mapMergeHandle(const json& msg) {
	// Implementation of map merge handling
}

bool TaskManager::initParameters() 
{
	if (!SharedParamsInterface::initialize()) {
		RCLCPP_INFO(node_->get_logger(), "Failed to initialize shared memory interface");
		return false;
	}

	try { 
		auto params = SharedParamsInterface::getNodeParams("shared_parameters");
		if (params.empty() || (params.find("error_code") != params.end() && params["error_code"].getValue<int>() > 0)) {
			RCLCPP_ERROR(node_->get_logger(), "Error get shared parameters");
			return false;
		} 

		params = SharedParamsInterface::getNodeParams("VDA5050Protocol");
		for (const auto& [name, value] : params) {
			RCLCPP_INFO_STREAM(node_->get_logger(), "get param:" << name << ", value:" << value.toString());
			switch(value.type) {
				case SharedParamsInterface::ParamValue::Type::INT:
					node_->declare_parameter(name, value.getValue<int>());
					break;
				case SharedParamsInterface::ParamValue::Type::DOUBLE:
					node_->declare_parameter(name, value.getValue<double>());
					break;
				case SharedParamsInterface::ParamValue::Type::STRING:
					node_->declare_parameter(name, value.getValue<std::string>());
					break;
				case SharedParamsInterface::ParamValue::Type::BOOL:
					node_->declare_parameter(name, value.getValue<bool>());
					break;
				default:
					RCLCPP_WARN_STREAM(node_->get_logger(), "Unsupported parameter type for " << name);
				break;
			}
		}
	} catch (const std::exception& e) {
		RCLCPP_ERROR_STREAM(node_->get_logger(), "Error reading parameters: " << e.what());
		return false;
	}
	return true;
}

